/**
  ******************************************************************************
  * @file    main.c 
  * @author  MCD Application Team
  * @version V1.1.0
  * @date    04-April-2014
  * @brief   Main program body
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "main.h"

/* Private typedef -----------------------------------------------------------*/
typedef struct {
  uint8_t group;
  uint8_t group_old;
  uint8_t safety;
  uint8_t leds[NO_OF_OUTPUTS];
  uint8_t pwm[4];
  uint8_t aux[4];
} state_struct;
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
state_struct state;

extern __IO uint8_t AnalogInputs[16];
uint8_t cur_left_vertical, cur_left_horizontal;
uint8_t cur_right_vertical, cur_right_horizontal;
uint8_t cur_left_track, cur_right_track;
uint8_t cur_drill_speed, cur_aux_adc;
uint8_t cur_karetka_speed;
uint8_t cur_tower_speed;
__IO uint8_t RigTemperature, RigConnectOK;
__IO uint32_t RigNonresponseCounter;

uint8_t cur_camera_rec;

extern __IO uint8_t timer_RS485DE_enable;
extern __IO uint8_t timer_RS485DE_disable;

uint8_t blink_cnt = 0, blink_pol = 0;

/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/

/**
  * @brief  Main program.
  * @param  None
  * @retval None
  */
int main(void)
{
  /*!< At this stage the microcontroller clock setting is already configured, 
       this is done through SystemInit() function which is called from startup
       file (startup_stm32f30x.s) before to branch to application main.
       To reconfigure the default setting of SystemInit() function, refer to
       system_stm32f30x.c file
     */ 
  RCC_ClocksTypeDef RCC_Clocks;
 
  GPIO_Config();
  ADC_Config();
  TIM_Config();
  USART_Config();
  
  /* SysTick end of count event each 1ms */
  RCC_GetClocksFreq(&RCC_Clocks);
  SysTick_Config(RCC_Clocks.HCLK_Frequency / SYSTICK_HZ);
  
  state.safety = 0;

  /* Infinite loop */
  while (1)
  {
  }
}

uint8_t isAround (uint32_t value, uint32_t reference, uint32_t ok_deviation)
{
  if (((value - reference) < ok_deviation) || (reference - value) < ok_deviation)
  {
    return 1;
  }
  else
  {
    return 0;
  }
}

uint8_t scale_pwm(uint8_t value, float scale)
{
  uint8_t unsigned_value;
  int16_t signed_value;

  unsigned_value = value;
  signed_value = unsigned_value - 127;
  signed_value = signed_value * scale;

  unsigned_value = signed_value + 127;

  return unsigned_value;
}

void calculate_groups (void)
{
  uint8_t grp_yel, grp_red, grp_grn;

  state.group_old = state.group;

  grp_yel = GPIO_GetInputState(IN_GRP1_YELLOW);
  grp_red = GPIO_GetInputState(IN_GRP2_RED);
  grp_grn = GPIO_GetInputState(IN_GRP3_GREEN);

  state.group = 0;
  
  // If safety was 0
  if (!state.safety)
  {
    if (!grp_yel && !grp_red && !grp_grn)
    {
      // If all group switches are off, release safety
      state.safety = 1;
    }
  }
  
  //state.safety = 1;
  state.leds[OUT_GRP1_YELLOW_LED] = 0;
  state.leds[OUT_GRP2_RED_LED] = 0;
  state.leds[OUT_GRP3_GREEN_LED] = 0;
  
  // Check for multiple enabled switches
  if ((grp_yel && grp_red) || (grp_yel && grp_grn) || (grp_red && grp_grn))
  {
    if (grp_yel) state.leds[OUT_GRP1_YELLOW_LED] = 2;
    if (grp_red) state.leds[OUT_GRP2_RED_LED] = 2;
    if (grp_grn) state.leds[OUT_GRP3_GREEN_LED] = 2;

    return;
  }

  // Group 1 (yellow)
  if (grp_yel)
  {
    state.group = 1;
    state.leds[OUT_GRP1_YELLOW_LED] = 1;
    cur_left_vertical    = AnalogInputs[ADC_YEL_L_V];
    cur_left_horizontal  = AnalogInputs[ADC_YEL_L_H];
    //cur_tower_speed      = scale_pwm(AnalogInputs[ADC_YEL_L_H], 0.5);
    cur_right_vertical   = AnalogInputs[ADC_YEL_R_V];
    cur_right_horizontal = AnalogInputs[ADC_YEL_R_H];
    //cur_karetka_speed    = scale_pwm(AnalogInputs[ADC_YEL_R_H], 0.5);

  }

  // Group 2 (red)
  if (grp_red)
  {
    int16_t calc_drive_speed, calc_drive_direction, calc_left_track, calc_right_track;

    state.group = 2;
    state.leds[OUT_GRP2_RED_LED] = 1;
    cur_left_vertical    = AnalogInputs[ADC_RED_L_V];
    cur_left_horizontal  = AnalogInputs[ADC_RED_L_H];
    cur_right_vertical   = AnalogInputs[ADC_RED_R_V];
    cur_right_horizontal = AnalogInputs[ADC_RED_R_H];

    // Assign drive speed and direction values
    calc_drive_speed      = cur_right_vertical;
    calc_drive_direction  = cur_right_horizontal;

    // Calculate speeds of left and right track
    calc_left_track       = calc_drive_speed + calc_drive_direction - 127;
    calc_right_track      = calc_drive_speed - calc_drive_direction + 127;

    // Values cannot go below 0 or above 255
    if(calc_left_track < 0)    calc_left_track  = 0;
    if(calc_left_track > 255)  calc_left_track  = 255;
    if(calc_right_track < 0)   calc_right_track = 0;
    if(calc_right_track > 255) calc_right_track = 255;

    // Update output track speed values
    cur_left_track  = calc_left_track;
    cur_right_track = calc_right_track;
  }

  // Group 3 (green) : drill, caret
  if (grp_grn)
  {
    state.group = 3;
    state.leds[OUT_GRP3_GREEN_LED] = 1;
    cur_left_vertical    = AnalogInputs[ADC_GRN_L_V];
    cur_left_horizontal  = 127;
    cur_right_vertical   = AnalogInputs[ADC_GRN_R_V];
    cur_right_horizontal = AnalogInputs[ADC_GRN_R_H];
		
		cur_drill_speed      = AnalogInputs[ADC_GRN_L_V];
		
		if (GPIO_GetInputState(IN_SW_DRILL_FWD))
		{
			if (cur_drill_speed < 127) cur_drill_speed = 127;
		}
		else if (GPIO_GetInputState(IN_SW_DRILL_RWD))
		{
			if (cur_drill_speed > 127) cur_drill_speed = 127;
		}
		else
		{
			cur_drill_speed = 127;
		}

    cur_aux_adc          = AnalogInputs[ADC_SPEED];
  }

  // If the group was switched, control is all joysticks are in center position
  if (state.group != state.group_old)
  {
    if(!isAround(cur_left_vertical, 127, SAFETY_MARGIN_JOYSTICK))
    {
      state.safety = 0;
    }
    if(!isAround(cur_left_horizontal, 127, SAFETY_MARGIN_JOYSTICK))
    {
      state.safety = 0;
    }
    if(!isAround(cur_right_vertical, 127, SAFETY_MARGIN_JOYSTICK))
    {
      state.safety = 0;
    }
    if(!isAround(cur_right_horizontal, 127, SAFETY_MARGIN_JOYSTICK))
    {
      state.safety = 0;
    }
  }
}

void calculate_aux_switches (void)
{
  state.leds[OUT_SW1_HYDROPUMP_LED] = 0;
  state.leds[OUT_SW2_WATERPUMP_LED] = 0;
  state.leds[OUT_SW3_LIGHT_LED] = 0;
  state.leds[OUT_SW4_CAMERA_LED] = 0;
  state.aux[0] = 0;
  state.aux[1] = 0;
  state.aux[2] = 0;
  state.aux[3] = 0;
	cur_camera_rec = 0;

  if (GPIO_GetInputState(IN_SW1_HYDROPUMP))
  {
    state.leds[OUT_SW1_HYDROPUMP_LED] = 1;
    state.aux[0] = 1;
  }
  else if (state.group)
  {
    // If hydropump is OFF and some group is ON, then flash both
    state.leds[OUT_SW1_HYDROPUMP_LED] = 3;
    
    // Flash LED of a group which is ON
    if(state.leds[OUT_GRP1_YELLOW_LED]) state.leds[OUT_GRP1_YELLOW_LED] = 2;
    if(state.leds[OUT_GRP2_RED_LED]) state.leds[OUT_GRP2_RED_LED] = 2;
    if(state.leds[OUT_GRP3_GREEN_LED]) state.leds[OUT_GRP3_GREEN_LED] = 2;
  }

  if (GPIO_GetInputState(IN_SW2_WATERPUMP))
  {
    state.leds[OUT_SW2_WATERPUMP_LED] = 1;
    state.aux[1] = 1;
  }

  if (GPIO_GetInputState(IN_SW3_LIGHT))
  {
    state.leds[OUT_SW3_LIGHT_LED] = 1;
    state.aux[2] = 1;
  }

  if (GPIO_GetInputState(IN_SW4_CAMERA))
  {
    state.leds[IN_SW4_CAMERA] = 1;
    state.aux[3] = 1;
  }
	
	if (GPIO_GetInputState(IN_SW_CAMERA_REC))
	{
		cur_camera_rec = 1;
	}
}

// Sets PWMs of LEDs
void set_led_pwms (void)
{
  uint8_t pwm_values[4];
  uint8_t i;

  if (state.group != state.group_old)
  {
    set_led_pwm_group(state.group);
  }

  switch (state.group)
  {
    case 3: // For drilling group (green) there is special arrangement
      pwm_values[0] = cur_drill_speed;
      pwm_values[1] = 127;
      pwm_values[2] = cur_right_vertical;
      pwm_values[3] = cur_right_horizontal;
      break;
    default:
      pwm_values[0] = cur_left_vertical;
      pwm_values[1] = cur_left_horizontal;
      pwm_values[2] = cur_right_vertical;
      pwm_values[3] = cur_right_horizontal;
  }

  // If safety is 0 then exaggerate LED brightness to see where the joystick is inclined
  if (!state.safety)
  {
    for (i = 0; i < 4; i++)
    {
      if (pwm_values[i] < (127 - SAFETY_MARGIN_JOYSTICK)) pwm_values[i] = blink_pol ? 0 : 127;
      if (pwm_values[i] > (127 + SAFETY_MARGIN_JOYSTICK)) pwm_values[i] = blink_pol ? 255 : 127;
    }
  }

  set_led_pwm_values(state.group, pwm_values);
}

// Set outputs for LEDs of switches
void set_sw_leds (void)
{
  uint8_t i;

  if (RigConnectOK)
  {
    // If connection to rig OK then just light the LED
    state.leds[OUT_POWER_LED] = 1;
  }
  else 
  {
    // If not OK, blink it
    state.leds[OUT_POWER_LED] = 1;
  }

  // If safety is 0 and some group is on, then blink its LED
  if (!state.safety)
  {
    if(state.leds[OUT_GRP1_YELLOW_LED] == 1) state.leds[OUT_GRP1_YELLOW_LED] = 2;
    if(state.leds[OUT_GRP2_RED_LED] == 1) state.leds[OUT_GRP2_RED_LED] = 2;
    if(state.leds[OUT_GRP3_GREEN_LED] == 1) state.leds[OUT_GRP3_GREEN_LED] = 2;
  }

  if(++blink_cnt == SCHEDULER_FREQUENCY/2)
  {
    blink_cnt = 0;
    blink_pol ^= 1;
  }

  // Update outputs for all LEDs
  for (i = 0; i < NO_OF_OUTPUTS; i++)
  {
    if (state.leds[i] == 0)      GPIO_SetOutput((GPIOOUTSignals_enum) i, 0);
    else if (state.leds[i] == 1) GPIO_SetOutput((GPIOOUTSignals_enum) i, 1);
    else if (state.leds[i] == 2) GPIO_SetOutput((GPIOOUTSignals_enum) i, blink_pol);
    else if (state.leds[i] == 3) GPIO_SetOutput((GPIOOUTSignals_enum) i, blink_pol ^ 1);
  }
}

void set_packet (void)
{
  /* Packet: 's' PWM1 PWM2 PWM3 PWM4 GRP,AUX CRC8 */
  uint8_t i;
  unsigned char str[7], grpaux = 0;
  uint8_t packet_group = state.safety ? state.group : 0;

  switch (packet_group)
  {
    case 1: //yellow group
      state.pwm[0] = cur_left_vertical;
      state.pwm[1] = cur_left_horizontal; //povorot bashni
      state.pwm[2] = cur_right_vertical;
      state.pwm[3] = cur_right_horizontal; // otkidnaja rama dlja kamery
      break;

    case 2: //red group
      state.pwm[0] = cur_left_vertical;
      state.pwm[1] = cur_left_horizontal;
      state.pwm[2] = cur_right_track;
      state.pwm[3] = 255 - cur_left_track; //wrong polarity on rig
      break;

    case 3: //green group
      state.pwm[0] = cur_right_vertical;
      state.pwm[1] = 255 - cur_drill_speed;
      state.pwm[2] = cur_right_horizontal;
      state.pwm[3] = cur_aux_adc;
      break;

    default:
      state.pwm[0] = 127;
      state.pwm[1] = 127;
      state.pwm[2] = 127;
      state.pwm[3] = 127;
      break;
  }

  for(i = 0; i < 4; i++)
  {
    grpaux |= ((state.aux[i] ? 1 : 0) << i);
  }
  grpaux |= packet_group << 4;
	
	if (cur_camera_rec)
	{
		grpaux |= 0x40;
	}

  str[0] = 's';
  str[1] = state.pwm[0];
  str[2] = state.pwm[1];
  str[3] = state.pwm[2];
  str[4] = state.pwm[3];
  str[5] = grpaux;
  str[6] = crc8(str, 6);

  USART_sendarray((uint8_t*) str, 7);
}

void scheduler (void)
{
  calculate_groups();
  calculate_aux_switches();
  set_led_pwms();
  set_sw_leds();
  set_packet();
}

#ifdef  USE_FULL_ASSERT

/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t* file, uint32_t line)
{ 
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */

  /* Infinite loop */
  while (1)
  {
  }
}
#endif

/**
  * @}
  */

